/*
 * @Description: incremental voxel map
 * @Autor: Zijie Chen
 * @Date: 2023-12-27 23:44:38
 */

#ifndef VOXEL_MAP_H_
#define VOXEL_MAP_H_

#include <execution>
#include <list>
#include <unordered_map>
#include <bitset>

#include <tbb/concurrent_hash_map.h>
#include <tbb/concurrent_vector.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_sort.h>

#include <boost/sort/spreadsort/integer_sort.hpp>

#include <glog/logging.h>

#include <Eigen/Core>

#include "sci_lio/point_type.h"


  // Grid 的结构体，主要用于增量体素地图（incremental voxel map）中的数据存储
struct Grid {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//用于确保结构体的内存与对齐要求相匹配，以便存储 Eigen 类型的数据，这样可以提高性能并避免内存错误
  //构造函数,指定每个网格可以存储的最大点数。points_array_ 被预留了两倍于 grid_max_points 的空间，以优化内存的分配和管理
  Grid(size_t grid_max_points) { points_array_.reserve(2 * grid_max_points); }

  //用于存储网格的哈希索引
  size_t hash_idx{0};
  //表示网格的中心点，使用三维向量表示
  Eigen::Vector3d centroid_ = Eigen::Vector3d::Zero();
  //表示当前网格点的协方差矩阵，初始化为零矩阵
  Eigen::Matrix3d cov_ = Eigen::Matrix3d::Zero();
  //表示协方差矩阵的逆，用于后续计算，初始化为零矩阵
  Eigen::Matrix3d inv_cov_ = Eigen::Matrix3d::Zero();
  //存储协方差的累积和，用于后续统计分析
  Eigen::Matrix3d cov_sum_ = Eigen::Matrix3d::Zero();
  //存储该网格中所有点的累积和，用于计算中心点
  Eigen::Vector3d points_sum_ = Eigen::Vector3d::Zero();
  //记录当前网格中点的数量
  size_t points_num_{0};
  //标记网格是否有效，用于有效性判断
  bool is_valid_{false};

  //用于存储该网格内的点，类型为三维向量的动态数组
  std::vector<Eigen::Vector3d> points_array_;

  std::bitset<27> features;

};

//用于存储三维点的坐标及其对应的哈希索引
struct point_hash_idx {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//为了确保结构体的内存与对齐要求相匹配，以支持 Eigen 库中的向量和矩阵运算，从而提高性能并避免潜在的内存错误
  //一个三维空间中的点的坐标
  Eigen::Vector3d point_;
  //该点的哈希索引，用于在哈希表中唯一标识和查找该点
  size_t hash_idx_;
  size_t hash_idx_z_;

  point_hash_idx() = default;
  //这个构造函数接受一个三维点和一个哈希索引作为参数，并将它们初始化到成员变量中
  point_hash_idx(Eigen::Vector3d point, size_t hash_idx)
      : point_(point)
      , hash_idx_(hash_idx)
      , hash_idx_z_(0) {}

  //重载了小于运算符 <，用于比较两个 point_hash_idx 对象。比较的依据是它们的 hash_idx_ 值。这样做通常是为了方便在排序或使用 STL 容器时的排序比较。
  bool operator<(const point_hash_idx& p) const {
    return (hash_idx_ < p.hash_idx_);
  }
};

// for KNN
//用于存储三维空间中的点及其与某个参考点之间的距离，以及该点的索引
struct point_distance {
 public:
  point_distance() = default;
  //一个三维向量 point，一个表示距离的 double 类型 distance，以及一个表示点索引的 size_t 类型 point_idx
  point_distance(const Eigen::Vector3d& point,
                 const double distance,
                 const size_t point_idx)
      : point_(point)
      , distance_(distance)
      , point_idx_(point_idx) {}
  //如果 p1 的距离小于 p2 的距离，则返回 true。这种方式通常用于排序或优先队列
  inline bool operator()(const point_distance& p1, const point_distance& p2) {
    return p1.distance_ < p2.distance_;
  }

  inline bool operator<(const point_distance& rhs) {
    return distance_ < rhs.distance_;
  }
  //表示一个三维空间中的点
  Eigen::Vector3d point_;
  //表示这个点到某个参考点的距离，初始化为 0.0
  double distance_{0.0};
  //表示该点的索引，初始化为 0
  size_t point_idx_{0};
};

//管理增量体素地图，为处理三维空间中的数据提供有效的存储和检索机制
class VoxelMap {
 public:
  struct Config {
    Config(){};

    //指定体素地图的分辨率，默认为 0.5
    double resolution{0.5};
    //定义用于查找点的搜索方法，这里默认值为 "NEARBY_7"
    std::string search_method = "NEARBY_7";
    //设置体素地图的最大容量，允许最多 5000000 个点
    size_t capacity{5000000};
    //grid_max_points: 每个网格中可以存储的最大点数，默认为 20
    size_t grid_max_points{20};
  };

  VoxelMap() = delete;

  VoxelMap(Config config = Config());
  //接受一个点云（CloudPtr类型指针），将其添加到体素地图中
  bool AddCloud(const CloudPtr& input_cloud_ptr);
  //给定一个点，并返回其周围网格的索引
  bool GetSurroundingGrids(const PointType& point, std::vector<size_t>& grids);
  //根据给定的三维点计算其哈希索引
  size_t ComputeHashIndex(const Eigen::Vector3d& point);
  size_t ComputeMaxZHashIndex(const Eigen::Vector3d& point);
  //计算并更新网格的协方差矩阵
  void ComputeCovariance(std::shared_ptr<Grid>& grid_ptr);
  //判断两个三维点是否属于同一个网格
  bool IsSameGrid(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2);
  //根据网格的哈希索引获取其中心点和协方差
  bool GetCentroidAndCovariance(const size_t hash_idx,
                                Eigen::Vector3d& centorid,
                                Eigen::Matrix3d& cov,
                                std::bitset<27>& features,
                                bool& is_valid);
  bool GetCentroidAndCovariance(const size_t hash_idx,
                                Eigen::Vector3d& centorid,
                                Eigen::Matrix3d& cov);
  //根据条件（如范围和 K 值）进行 K 最近邻查找并返回结果
  bool KNNByCondition(const Eigen::Vector3d& point,
                      const size_t K,
                      const double range,
                      std::vector<Eigen::Vector3d>& results);
  //返回当前体素地图中存储的网格数量
  size_t GetVoxelMapSize() { return voxel_map_.size(); }
  //存储点的增量变化的三维向量
  std::vector<Eigen::Vector3d> delta_P_;
  double resolution_{1.0};
  double inv_resolution_{1.0};
  //体素地图的最大容量，最多可存储 5000000 个点
  size_t capacity_{5000000};
  //每个网格中允许存储的最大点数，默认为 20
  size_t grid_max_points_{20};

  using MyHashMapVoxel = tbb::concurrent_hash_map<size_t, std::shared_ptr<Voxel>>;
  using MyHashMap = tbb::concurrent_hash_map<size_t, std::shared_ptr<Grid>>;
  using MyVector = tbb::concurrent_vector<std::shared_ptr<Grid>>;
  using MyAccessor = MyHashMap::accessor;
  using MyAccessorVoxel = MyHashMapVoxel::accessor;

  std::shared_ptr<MyHashMap> temp_voxel_map_ptr_;
  std::shared_ptr<MyVector> temp_voxel_array_ptr_;

  std::unordered_map<size_t,typename std::list<std::pair<size_t, std::shared_ptr<Grid>>>::iterator> voxel_map_;
  std::shared_ptr<MyHashMapVoxel> voxel_MaxZ_map_;
  std::list<std::pair<size_t, std::shared_ptr<Grid>>> grids_cache_;

  size_t HASH_P_{116101};
  size_t MAX_N_{10000000000};
  size_t invalid_point1;
  size_t invalid_point2;
  bool calculate_invalid;
  std::vector<Eigen::Vector3d> search_range_;
};

#endif